//解决锁问题 3条path
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <atomic>
#include <thread>
#include <rclcpp/executors/multi_threaded_executor.hpp>

class CombinedNode : public rclcpp::Node {
public:
    CombinedNode() : Node("combined_node"), force_sign(true), closest_index(0) {
        // 初始化力数据订阅
        force_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            "/force_data", 10,
            std::bind(&CombinedNode::force_callback, this, std::placeholders::_1));

        // 初始化MoveGroupInterface
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
            std::shared_ptr<rclcpp::Node>(this),  // 显式创建shared_ptr
            "zc4");
        
        RCLCPP_INFO(get_logger(), "节点初始化完成");
    }

    // 修改 start_execution 以接受路径索引
    void start_execution(int path_index) {
        std::thread([this, path_index]() {
            switch (path_index) {
                case 1:
                    executeCartesianPath1();
                    break;
                case 2:
                    executeCartesianPath2();
                    break;
                case 3:
                    executeCartesianPath3();
                    break;
                default:
                    RCLCPP_ERROR(get_logger(), "无效的路径索引: %d", path_index);
                    break;
            }
        }).detach();
    }

private:
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr force_sub_;
    std::atomic<bool> force_sign;
    std::atomic<size_t> closest_index;

    void force_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
        if (std::abs(msg->twist.linear.z) >= 5.0) {
            force_sign = false;
        } else {
            force_sign = true;
        }
    }

    bool returnToMiddle() {
        move_group_->setNamedTarget("middle");
        moveit::planning_interface::MoveGroupInterface::Plan home_plan;
        bool success = (move_group_->plan(home_plan) == moveit::core::MoveItErrorCode::SUCCESS);
        if (success) {
            move_group_->execute(home_plan);
            RCLCPP_INFO(get_logger(), "回到middle位置");
            return true;
        }
        RCLCPP_ERROR(get_logger(), "回到middle位置失败");
        return false;
    }

    bool continueCartesianPath(const std::vector<geometry_msgs::msg::Pose>& original_waypoints) {
        geometry_msgs::msg::Pose current_pose = move_group_->getCurrentPose().pose;
        std::vector<geometry_msgs::msg::Pose> remaining_waypoints;
        double min_distance = std::numeric_limits<double>::max();

        for (size_t i = 0; i < original_waypoints.size(); ++i) {
            double dx = current_pose.position.x - original_waypoints[i].position.x;
            double dy = current_pose.position.y - original_waypoints[i].position.y;
            double dz = current_pose.position.z - original_waypoints[i].position.z;
            double distance = std::sqrt(dx*dx + dy*dy + dz*dz);
            if (distance < min_distance) {
                min_distance = distance;
                closest_index = i;
            }
        }

        for (size_t i = closest_index + 1; i < original_waypoints.size(); ++i) {
            remaining_waypoints.push_back(original_waypoints[i]);
        }

        if (remaining_waypoints.empty()) return false;

        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan new_plan;
        double fraction = move_group_->computeCartesianPath(remaining_waypoints, 0.01, 0.0, new_plan.trajectory_);
        
        if (fraction >= 0.9) {
            move_group_->asyncExecute(new_plan);
            return true;
        }
        return false;
    }

    void executeCartesianPath1() {
        if (!returnToMiddle()) return;
        
        std::vector<geometry_msgs::msg::Pose> waypoints;
        // [保持原有waypoints定义不变]
        geometry_msgs::msg::Pose pose1;
        pose1.position.x = 0.1038815445000545;
        pose1.position.y = 0.05599530875132761;
        pose1.position.z = 0.287593164792294;
        pose1.orientation.x = -7.443283672575919e-06;
        pose1.orientation.y = 7.57226530269504e-05;
        pose1.orientation.z = 0.12184966603307737;
        pose1.orientation.w = 0.9925485646044253;
        waypoints.push_back(pose1);
    
        geometry_msgs::msg::Pose pose2;
        pose2.position.x = 0.08365577107241515;
        pose2.position.y = 0.1086784487646846;
        pose2.position.z = 0.2876367813618629;
        pose2.orientation.x = 1.760144636797022e-06;
        pose2.orientation.y = 5.322223642645702e-07;
        pose2.orientation.z = 0.24192347154193786;
        pose2.orientation.w = 0.970295333346356;
        waypoints.push_back(pose2);
    
        geometry_msgs::msg::Pose pose3;
        pose3.position.x = -0.024580082964917316;
        pose3.position.y = 0.05115885226571315;
        pose3.position.z = 0.14427007528930358;
        pose3.orientation.x = -0.09252984740619437;
        pose3.orientation.y = 0.3713226058418401;
        pose3.orientation.z = 0.22355615291928813;
        pose3.orientation.w = 0.8964264589032053;
        waypoints.push_back(pose3);
    
        geometry_msgs::msg::Pose pose4;
        pose4.position.x = 0.0059124806471319985;
        pose4.position.y = 0.06736960543726173;
        pose4.position.z = 0.0851790937420965;
        pose4.orientation.x = -0.0787478196797887;
        pose4.orientation.y = 0.3158430604116397;
        pose4.orientation.z = 0.22880413581912243;
        pose4.orientation.w = 0.9174369784990976;
        waypoints.push_back(pose4);
    
        geometry_msgs::msg::Pose pose5;
        pose5.position.x = 0.014876747557903927;
        pose5.position.y = 0.07209511922660344;
        pose5.position.z = 0.069480923286174;
        pose5.orientation.x = -0.08464301863551622;
        pose5.orientation.y = 0.33968405068949703;
        pose5.orientation.z = 0.22656194017166972;
        pose5.orientation.w = 0.9089114326319655;
        waypoints.push_back(pose5);
    
        geometry_msgs::msg::Pose pose6;
        pose6.position.x = 0.032490128976992244;
        pose6.position.y = 0.08148829798108001;
        pose6.position.z = 0.08403494444521113;
        pose6.orientation.x = -0.0686576561182485;
        pose6.orientation.y = 0.275514230486597;
        pose6.orientation.z = 0.2319904963985905;
        pose6.orientation.w = 0.9303539351432134;
        waypoints.push_back(pose6);
    
        geometry_msgs::msg::Pose pose7;
        pose7.position.x = 0.044606693998343105;
        pose7.position.y = 0.08792626579373217;
        pose7.position.z = 0.08060679535604816;
        pose7.orientation.x = -0.06055302337925643;
        pose7.orientation.y = 0.24294582246096078;
        pose7.orientation.z = 0.23423143748854183;
        pose7.orientation.w = 0.9393861252969676;
        waypoints.push_back(pose7);
    
        geometry_msgs::msg::Pose pose8;
        pose8.position.x = 0.08050738757562798;
        pose8.position.y = 0.10705084955998226;
        pose8.position.z = 0.10661040040187303;
        pose8.orientation.x = -0.03783271841333385;
        pose8.orientation.y = 0.1517922939389349;
        pose8.orientation.z = 0.23904254249342105;
        pose8.orientation.w = 0.9583248133052252;
        waypoints.push_back(pose8);
    
        geometry_msgs::msg::Pose pose9;
        pose9.position.x = 0.09048433218830905;
        pose9.position.y = 0.11233859740087646;
        pose9.position.z = 0.15069511834155236;
        pose9.orientation.x = -0.006308112047193989;
        pose9.orientation.y = 0.025320410227902945;
        pose9.orientation.z = 0.24191090140946767;
        pose9.orientation.w = 0.9699475245226156;
        waypoints.push_back(pose9);
    
        geometry_msgs::msg::Pose pose10;
        pose10.position.x = 0.10781150531469692;
        pose10.position.y = 0.057003964556817374;
        pose10.position.z = 0.1530723139619223;
        pose10.orientation.x = 9.186180341598707e-06;
        pose10.orientation.y = -5.977016834688678e-05;
        pose10.orientation.z = 0.12192029901858292;
        pose10.orientation.w = 0.9925398919088142;
        waypoints.push_back(pose10);
    
        geometry_msgs::msg::Pose pose11;
        pose11.position.x = 0.11075168589927481;
        pose11.position.y = -1.091277667118024e-05;
        pose11.position.z = 0.2875988062936282;
        pose11.orientation.x = 1.8382147442206865e-06;
        pose11.orientation.y = 8.132110012412454e-05;
        pose11.orientation.z = -2.4453694472133078e-05;
        pose11.orientation.w = 0.9999999963927584;
        waypoints.push_back(pose11);
    
        // geometry_msgs::msg::Pose pose12;
        // pose12.position.x = 0.3035704582748756;
        // pose12.position.y = -4.6019088696090045e-05;
        // pose12.position.z = 0.30536264212765296;
        // pose12.orientation.x = 1.8345570569793093e-06;
        // pose12.orientation.y = -7.180863568924419e-05;
        // pose12.orientation.z = -6.589662785214818e-05;
        // pose12.orientation.w = 0.9999999952488945;
        // waypoints.push_back(pose12);

        move_group_->stop();
        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
        double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
        
        if (fraction < 0.9) {
            RCLCPP_ERROR(get_logger(), "初始路径规划失败，完成度: %f", fraction);
            return;
        }

        RCLCPP_INFO(get_logger(), "初始路径规划成功，开始执行...");
        move_group_->asyncExecute(cartesian_plan);
        bool is_stopped = false;
        std::atomic<bool> motion_complete(false);

        while (rclcpp::ok() && !motion_complete) {
            if (!force_sign.load()) {
                if (!is_stopped) {
                    RCLCPP_INFO(get_logger(), "检测到过大力，尝试停止运动...");
                    move_group_->stop();
                    is_stopped = true;
                    RCLCPP_INFO(get_logger(), "运动已停止");
                }
            } else {
                if (is_stopped) {
                    RCLCPP_INFO(get_logger(), "力信号恢复，尝试继续运动...");
                    if (continueCartesianPath(waypoints)) {
                        RCLCPP_INFO(get_logger(), "已恢复运动");
                        is_stopped = false;
                    } else {
                        RCLCPP_INFO(get_logger(), "无法继续运动或路径已完成");
                        motion_complete = true;
                        break;
                    }
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        if (motion_complete.load()) {
            RCLCPP_INFO(get_logger(), "运动执行循环结束（可能已完成或无法继续）");
        } else if (!rclcpp::ok()) {
            RCLCPP_INFO(get_logger(), "运动执行循环因 rclcpp 关闭而结束");
        }
        move_group_->stop();
        RCLCPP_INFO(get_logger(), "executeCartesianPath1 结束");
        rclcpp::shutdown();
    }
    
    void executeCartesianPath2() {
        if (!returnToMiddle()) return;
    /* #region  */
        std::vector<geometry_msgs::msg::Pose> waypoints;
        // 目标点1 0/-50/90/15/0
        geometry_msgs::msg::Pose pose1;
        pose1.position.x = -0.011801240508450508;
        pose1.position.y = -1.6717636845861036e-05;
        pose1.position.z = 0.14428133950504773;
        pose1.orientation.x = -2.3927732160852933e-06;
        pose1.orientation.y = 0.3826546744537482;
        pose1.orientation.z = -5.627848297886214e-05;
        pose1.orientation.w = 0.9238914421866365;
        waypoints.push_back(pose1);

        // 目标点2 0/-70/120/-12/0
        geometry_msgs::msg::Pose pose2;
        pose2.position.x = 0.02270275459045043;
        pose2.position.y = -3.0206178897082266e-05;
        pose2.position.z = 0.08515341454770431;
        pose2.orientation.x = -3.0504780108955957e-05;
        pose2.orientation.y = 0.32557952141613367;
        pose2.orientation.z = -8.924993019861189e-05;
        pose2.orientation.w = 0.9455146568606697;
        waypoints.push_back(pose2);

        // 目标点3 0/-65/121/-18/0
        geometry_msgs::msg::Pose pose3;
        pose3.position.x = 0.03716382693672583;
        pose3.position.y = -1.5665732298609626e-05;
        pose3.position.z = 0.08467326062547971;
        pose3.orientation.x = -2.6268154351170503e-05;
        pose3.orientation.y = 0.3256253211597455;
        pose3.orientation.z = -4.6484770819418694e-05;
        pose3.orientation.w = 0.945498888084361;
        waypoints.push_back(pose3);

        // 目标点4 0/-68/128/-27/0
        geometry_msgs::msg::Pose pose4;
        pose4.position.x = 0.05279465803518045;
        pose4.position.y = 2.85229202356785e-05;
        pose4.position.z = 0.08403403724734092;
        pose4.orientation.x = -1.0133215588657257e-05;
        pose4.orientation.y = 0.2839328542037646;
        pose4.orientation.z = 7.950130493131293e-05;
        pose4.orientation.w = 0.9588441624584073;
        waypoints.push_back(pose4);

        // 目标点5 0/-70/135/-33/0 cz 0/-73/135/-34/0
        geometry_msgs::msg::Pose pose5;
        pose5.position.x = 0.06279208075096127;
        pose5.position.y = 8.436405029428977e-06;
        pose5.position.z = 0.08359961953583876;
        pose5.orientation.x = -1.421938048450076e-05;
        pose5.orientation.y = 0.2418572307363304;
        pose5.orientation.z = 1.8897337572279438e-05;
        pose5.orientation.w = 0.9703118464603292;
        waypoints.push_back(pose5);

        // 目标点6 0/-70/141/-50/0
        geometry_msgs::msg::Pose pose6;
        pose6.position.x = 0.08872055239306093;
        pose6.position.y = 0.00012314349764190854;
        pose6.position.z = 0.09403262420395148;
        pose6.orientation.x = -1.2451783763174253e-05;
        pose6.orientation.y = 0.1823097069171027;
        pose6.orientation.z = 0.0003162268136817005;
        pose6.orientation.w = 0.9832411050242742;
        waypoints.push_back(pose6);

        // 目标点7 0/-72/150/-78/0 举平 cz 0/-72/155/-85/0
        geometry_msgs::msg::Pose pose7;
        pose7.position.x = 0.1175734673974146;
        pose7.position.y = 1.2029243031720646e-05;
        pose7.position.z = 0.14823841510901875;
        pose7.orientation.x = 1.4323529733784588e-06;
        pose7.orientation.y = -0.01745669931009907;
        pose7.orientation.z = 3.443876281589146e-05;
        pose7.orientation.w = 0.9998476196206685;
        waypoints.push_back(pose7);

        // 目标点9 16/-72/150/-78/0 举平移
        geometry_msgs::msg::Pose pose9;
        pose9.position.x = 0.10568869680086797;
        pose9.position.y = 0.06496941623114455;
        pose9.position.z = 0.1529569397066705;
        pose9.orientation.x = -2.5174032480959837e-05;
        pose9.orientation.y = 0.0001925602901853463;
        pose9.orientation.z = 0.13929909717987762;
        pose9.orientation.w = 0.9902503339114176;
        waypoints.push_back(pose9);

        // 目标点10 middle
        geometry_msgs::msg::Pose pose10;
        pose10.position.x = 0.11075168589927481;
        pose10.position.y = -1.091277667118024e-05;
        pose10.position.z = 0.2875988062936282;
        pose10.orientation.x = 1.8382147442206865e-06;
        pose10.orientation.y = 8.132110012412454e-05;
        pose10.orientation.z = -2.4453694472133078e-05;
        pose10.orientation.w = 0.9999999963927584;
        waypoints.push_back(pose10);

        // // 目标点11 0/40/0/-40/0
        // geometry_msgs::msg::Pose pose11;
        // pose11.position.x = 0.3035704582748756;
        // pose11.position.y = -4.6019088696090045e-05;
        // pose11.position.z = 0.30536264212765296;
        // pose11.orientation.x = 1.8345570569793093e-06;
        // pose11.orientation.y = -7.180863568924419e-05;
        // pose11.orientation.z = -6.589662785214818e-05;
        // pose11.orientation.w = 0.9999999952488945;
        // waypoints.push_back(pose11);
    /* #endregion */

        move_group_->stop();
        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
        double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
        
        if (fraction < 0.9) {
            RCLCPP_ERROR(get_logger(), "初始路径规划失败，完成度: %f", fraction);
            return;
        }

        RCLCPP_INFO(get_logger(), "初始路径规划成功，开始执行...");
        move_group_->asyncExecute(cartesian_plan);
        bool is_stopped = false;
        std::atomic<bool> motion_complete(false);

        while (rclcpp::ok() && !motion_complete) {
            if (!force_sign.load()) {
                if (!is_stopped) {
                    RCLCPP_INFO(get_logger(), "检测到过大力，尝试停止运动...");
                    move_group_->stop();
                    is_stopped = true;
                    RCLCPP_INFO(get_logger(), "运动已停止");
                }
            } else {
                if (is_stopped) {
                    RCLCPP_INFO(get_logger(), "力信号恢复，尝试继续运动...");
                    if (continueCartesianPath(waypoints)) {
                        RCLCPP_INFO(get_logger(), "已恢复运动");
                        is_stopped = false;
                    } else {
                        RCLCPP_INFO(get_logger(), "无法继续运动或路径已完成");
                        motion_complete = true;
                        break;
                    }
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        if (motion_complete.load()) {
            RCLCPP_INFO(get_logger(), "运动执行循环结束（可能已完成或无法继续）");
        } else if (!rclcpp::ok()) {
            RCLCPP_INFO(get_logger(), "运动执行循环因 rclcpp 关闭而结束");
        }
        move_group_->stop();
        RCLCPP_INFO(get_logger(), "executeCartesianPath2 结束");
        rclcpp::shutdown();
}

    void executeCartesianPath3() {
        if (!returnToMiddle()) return;
        /* #region  */
        std::vector<geometry_msgs::msg::Pose> waypoints;
        // 目标点1 -14/-45/90/-45/0
        geometry_msgs::msg::Pose pose1;
        pose1.position.x = 0.1038794732882471;
        pose1.position.y = -0.055993214192435026;
        pose1.position.z = 0.2876041751758597;
        pose1.orientation.x = 7.895216365982998e-06;
        pose1.orientation.y = 4.925607556231088e-05;
        pose1.orientation.z = -0.12184804269604492;
        pose1.orientation.w = 0.9925487655539387;
        waypoints.push_back(pose1);

        // 目标点2 -28/-45/90/-45/0
        geometry_msgs::msg::Pose pose2;
        pose2.position.x = 0.08367758643029065;
        pose2.position.y = -0.1086808498137246;
        pose2.position.z = 0.2875887574546225;
        pose2.orientation.x = 1.927121538709558e-05;
        pose2.orientation.y = 6.971049619118313e-05;
        pose2.orientation.z = -0.24189446329796155;
        pose2.orientation.w = 0.97030256280959;
        waypoints.push_back(pose2);

        // 目标点3 -28/-50/80/15/0
        geometry_msgs::msg::Pose pose3;
        pose3.position.x = -0.024548290170542997;
        pose3.position.y = -0.051114431259759716;
        pose3.position.z = 0.14426120700169354;
        pose3.orientation.x = 0.09259218221194593;
        pose3.orientation.y = 0.37132642847766817;
        pose3.orientation.z = -0.223401705139517;
        pose3.orientation.w = 0.8964569423279657;
        waypoints.push_back(pose3);

        // 目标点4 -28/-70/120/-12/0
        geometry_msgs::msg::Pose pose4;
        pose4.position.x = 0.0059175229032937815;
        pose4.position.y = -0.06733821725529181;
        pose4.position.z = 0.08516913944763507;
        pose4.orientation.x = 0.07877489671301727;
        pose4.orientation.y = 0.3158618841182072;
        pose4.orientation.z = -0.2287160122165187;
        pose4.orientation.w = 0.9174501466373653;
        waypoints.push_back(pose4);

        // 目标点5 -28/-65/121/-17/0
        geometry_msgs::msg::Pose pose5;
        pose5.position.x = 0.014856622895562388;
        pose5.position.y = -0.0721048590648025;
        pose5.position.z = 0.06944823076037311;
        pose5.orientation.x = 0.08474302001326812;
        pose5.orientation.y = 0.3397336582584099;
        pose5.orientation.z = -0.22661422321503966;
        pose5.orientation.w = 0.908870538548826;
        waypoints.push_back(pose5);

        // 目标点6 -28/-68/128/-27/0
        geometry_msgs::msg::Pose pose6;
        pose6.position.x = 0.032492179697122675;
        pose6.position.y = -0.0814728216582047;
        pose6.position.z = 0.08401846064911692;
        pose6.orientation.x = 0.06864384957945104;
        pose6.orientation.y = 0.27555892205004473;
        pose6.orientation.z = -0.23195179933670168;
        pose6.orientation.w = 0.9303513665158974;
        waypoints.push_back(pose6);

        // 目标点7 -28/-70/135/-36/0
        geometry_msgs::msg::Pose pose7;
        pose7.position.x = 0.0446287643769399;
        pose7.position.y = -0.08791412462470531;
        pose7.position.z = 0.08061218194929645;
        pose7.orientation.x = 0.06051325339911822;
        pose7.orientation.y = 0.242909963020376;
        pose7.orientation.z = -0.23418814928531106;
        pose7.orientation.w = 0.9394087538248804;
        waypoints.push_back(pose7);

        // 目标点8 -28/-70/141/-50/0
        geometry_msgs::msg::Pose pose8;
        pose8.position.x = 0.08054424192783191;
        pose8.position.y = -0.10699752705986922;
        pose8.position.z = 0.10661899188484153;
        pose8.orientation.x = 0.03779276087929698;
        pose8.orientation.y = 0.15177439309731033;
        pose8.orientation.z = -0.2389260449080508;
        pose8.orientation.w = 0.9583582763714517;
        waypoints.push_back(pose8);

        // 目标点9 -28/-72/150/-78/0 举平
        geometry_msgs::msg::Pose pose9;
        pose9.position.x = 0.09049845893474838;
        pose9.position.y = -0.11230423464398587;
        pose9.position.z = 0.1506514470154047;
        pose9.orientation.x = 0.006333520542832988;
        pose9.orientation.y = 0.025396788065378907;
        pose9.orientation.z = -0.24180305139626612;
        pose9.orientation.w = 0.969972254246971;

        // 目标点10 -14/-72/150/-78/0 举平移
        geometry_msgs::msg::Pose pose10;
        pose10.position.x = 0.1078137015424794;
        pose10.position.y = -0.057002013797603834;
        pose10.position.z = 0.15304097497404734;
        pose10.orientation.x = 7.721304036228014e-06;
        pose10.orientation.y = 4.7830876074295094e-05;
        pose10.orientation.z = -0.12190999444880327;
        pose10.orientation.w = 0.9925411582932376;
        waypoints.push_back(pose10);

        // 目标点11 middle
        geometry_msgs::msg::Pose pose11;
        pose11.position.x = 0.11075168589927481;
        pose11.position.y = -1.091277667118024e-05;
        pose11.position.z = 0.2875988062936282;
        pose11.orientation.x = 1.8382147442206865e-06;
        pose11.orientation.y = 8.132110012412454e-05;
        pose11.orientation.z = -2.4453694472133078e-05;
        pose11.orientation.w = 0.9999999963927584;
        waypoints.push_back(pose11);

        // // 目标点12 0/40/0/-40/0
        // geometry_msgs::msg::Pose pose12;
        // pose12.position.x = 0.3035704582748756;
        // pose12.position.y = -4.6019088696090045e-05;
        // pose12.position.z = 0.30536264212765296;
        // pose12.orientation.x = 1.8345570569793093e-06;
        // pose12.orientation.y = -7.180863568924419e-05;
        // pose12.orientation.z = -6.589662785214818e-05;
        // pose12.orientation.w = 0.9999999952488945;
        // waypoints.push_back(pose12);
        /* #endregion */

        move_group_->stop();
        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
        double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
        
        if (fraction < 0.9) {
            RCLCPP_ERROR(get_logger(), "初始路径规划失败，完成度: %f", fraction);
            return;
        }

        RCLCPP_INFO(get_logger(), "初始路径规划成功，开始执行...");
        move_group_->asyncExecute(cartesian_plan);
        bool is_stopped = false;
        std::atomic<bool> motion_complete(false);

        while (rclcpp::ok() && !motion_complete) {
            if (!force_sign.load()) {
                if (!is_stopped) {
                    RCLCPP_INFO(get_logger(), "检测到过大力，尝试停止运动...");
                    move_group_->stop();
                    is_stopped = true;
                    RCLCPP_INFO(get_logger(), "运动已停止");
                }
            } else {
                if (is_stopped) {
                    RCLCPP_INFO(get_logger(), "力信号恢复，尝试继续运动...");
                    if (continueCartesianPath(waypoints)) {
                        RCLCPP_INFO(get_logger(), "已恢复运动");
                        is_stopped = false;
                    } else {
                        RCLCPP_INFO(get_logger(), "无法继续运动或路径已完成");
                        motion_complete = true;
                        break;
                    }
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        if (motion_complete.load()) {
            RCLCPP_INFO(get_logger(), "运动执行循环结束（可能已完成或无法继续）");
        } else if (!rclcpp::ok()) {
            RCLCPP_INFO(get_logger(), "运动执行循环因 rclcpp 关闭而结束");
        }
        move_group_->stop();
        RCLCPP_INFO(get_logger(), "executeCartesianPath3 结束");
        rclcpp::shutdown();
    }

};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CombinedNode>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);

    node->start_execution(1); 

    RCLCPP_INFO(node->get_logger(), "开始执行器 spin");
    executor.spin();

    RCLCPP_INFO(node->get_logger(), "执行器 spin 结束");
    rclcpp::shutdown();
    return 0;
}